#pragma config(Hubs,  S1, HTMotor,  HTMotor,  none,     none)
#pragma config(Hubs,  S2, HTServo,  HTMotor,  none,     none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     ,               sensorI2CMuxController)
#pragma config(Motor,  mtr_S1_C1_1,     rightFront,    tmotorTetrix, PIDControl, reversed)
#pragma config(Motor,  mtr_S1_C1_2,     rightBack,     tmotorTetrix, PIDControl, reversed)
#pragma config(Motor,  mtr_S1_C2_1,     leftBack,      tmotorTetrix, PIDControl)
#pragma config(Motor,  mtr_S1_C2_2,     leftFront,     tmotorTetrix, PIDControl)
#pragma config(Motor,  mtr_S2_C2_1,     beaterBar,     tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S2_C2_2,     lift,          tmotorTetrix, openLoop, encoder)
#pragma config(Servo,  srvo_S2_C1_1,    backRightServo,       tServoStandard)
#pragma config(Servo,  srvo_S2_C1_2,    backLeftServo,        tServoStandard)
#pragma config(Servo,  srvo_S2_C1_3,    topServo,             tServoStandard)
#pragma config(Servo,  srvo_S2_C1_4,    fghjk,                tServoStandard)
#pragma config(Servo,  srvo_S2_C1_5,    sdfgbhnm,             tServoStandard)
#pragma config(Servo,  srvo_S2_C1_6,    sdfghj,               tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

task main()
{
	float s = 0;

	while(true){
		/*
		if(nNxtButtonPressed != -1){
			if(nNxtButtonPressed == 1){
				s += 5;
				wait1Msec(200);
			}else if(nNxtButtonPressed == 2){
				s -= 5;
				wait1Msec(200);
			}else if(nNxtButtonPressed == 3){

			}
			servo[topServo] = s;
			nxtDisplayTextLine(3, "%d servo's", s);

		}
		*/
		if(nNxtButtonPressed != -1){
			if(nNxtButtonPressed == 1){
				servo[topServo] = 100;
			}else if(nNxtButtonPressed == 2){
				servo[topServo] = 235;
			}else if(nNxtButtonPressed == 3){

			}



		}
	}


}
